﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;


namespace cs5643.rigidbody
{
    public class AABBTree
    {
        private static int maxDepth = 10;
        private static int minBlocks = 3;

        private Pnt2D min, max;
        private AABBTree left, right;
        private List<Block> blocks;
        private int depth = 0;



        public static List<Pair<Block, Block>> findCollisions(AABBTree t1, AABBTree t2, RigidBody b1, RigidBody b2) {
		List<Pair<Block, Block>> rv = new List<Pair<Block, Block>>();
		if (hasSeperator(t1, t2, b1, b2)) {
			return rv;
		}
		
		if (t1.isLeaf() && t2.isLeaf()) {
			foreach (Block s1 in t1.blocks) {
				foreach (Block s2 in t2.blocks) {
					Pnt2D  piW = new Pnt2D(0,0);
					Pnt2D  pjW = new Pnt2D(0,0);
				    piW.Set(s1.p);//body coords
				    b1.do_transformB2W( ref piW);//-->world
					pjW.Set(s2.p);//body coords
                    b2.do_transformB2W( ref pjW);//-->world
                    Pnt2D v = Pnt2D.Sub(pjW, piW);
					double dist     = v.Length + Constants.eps10;		
					double sumRadii = 1.41*(s1.h + s2.h);
					double penDepth = sumRadii - dist;
					if (penDepth > 0) rv.Add(new Pair<Block, Block>(s1, s2));
				}
			}
			return rv;
		}
		
		if (t1.isLeaf() && !t2.isLeaf()) {
			rv.AddRange(findCollisions(t1, t2.left, b1, b2));
			rv.AddRange(findCollisions(t1, t2.right, b1, b2));
			return rv;
		}
		
		if (!t1.isLeaf() && t2.isLeaf()) {
			rv.AddRange(findCollisions(t1.left, t2, b1, b2));
			rv.AddRange(findCollisions(t1.right, t2, b1, b2));
			return rv;
		}
		
		if (!t1.isLeaf() && !t2.isLeaf()) {
			if (t1.blocks.Count > t2.blocks.Count) {
				rv.AddRange(findCollisions(t1.left, t2, b1, b2));
				rv.AddRange(findCollisions(t1.right, t2, b1, b2));
			} else {
				rv.AddRange(findCollisions(t1, t2.left, b1, b2));
				rv.AddRange(findCollisions(t1, t2.right, b1, b2));
			}
			return rv;
		}
			
		return null;
	}

        private static bool hasSeperator(AABBTree t1, AABBTree t2, RigidBody b1, RigidBody b2) {
		Pnt2D[] testers = new Pnt2D[4];
		testers[0] = new Pnt2D(0, 1);
        b1.do_transformB2W( ref testers[0]);
		testers[1] = new Pnt2D(1, 0);
        b1.do_transformB2W( ref testers[1]);
		testers[2] = new Pnt2D(0, 1);
        b2.do_transformB2W( ref testers[2]);
		testers[3] = new Pnt2D(1, 0);
        b2.do_transformB2W( ref testers[3]);
		
		Pnt2D[] t1p = new Pnt2D[4];
		Pnt2D[] t2p = new Pnt2D[4];
		
		t1p[0] = new Pnt2D(t1.min);
		t1p[1] = new Pnt2D(t1.max);
		t1p[2] = new Pnt2D(t1.max.x, t1.min.y);
		t1p[3] = new Pnt2D(t1.min.x, t1.max.y);
		
		t2p[0] = new Pnt2D(t2.min);
		t2p[1] = new Pnt2D(t2.max);
		t2p[2] = new Pnt2D(t2.max.x, t2.min.y);
		t2p[3] = new Pnt2D(t2.min.x, t2.max.y);
		
		for (int i = 0; i < 4 ; i++) {
			b1.do_transformB2W( ref t1p[i]);
			b2.do_transformB2W( ref t2p[i]);
		}
		
		bool foundSeperator = false;
		for (int i = 0 ; i < 4 ; i++) {
			double minB1 = Double.MaxValue;
			double minB2 = Double.MaxValue;
			double maxB1 = -Double.MaxValue;
			double maxB2 = -Double.MaxValue;
			foundSeperator = true;
			for (int e = 0; e < 4 ; e++) {
				minB1 = Math.Min(minB1, t1p[e].Dot(testers[i]));
				maxB1 = Math.Max(maxB1, t1p[e].Dot(testers[i]));
				minB2 = Math.Min(minB2, t2p[e].Dot(testers[i]));
				maxB2 = Math.Max(maxB2, t2p[e].Dot(testers[i]));
			}
			
			if (minB1 <= maxB2 && minB2 <= maxB1){
				foundSeperator = false;
				continue;
			}
			
			if (foundSeperator) return true;
		}
		return false;
		
	}

        private bool isLeaf()
        {

            return (left == null && right == null);

        }

        private void setBounds(List<Block> rigid) {
		
		double minx = Double.MaxValue;
		double miny = Double.MaxValue;
		double maxx = -Double.MaxValue;
		double maxy = -Double.MaxValue;
		foreach (Block b in rigid) {	
			minx = Math.Min(b.p.x - b.h, minx);
			miny = Math.Min(b.p.y - b.h, miny);
			maxx = Math.Max(b.p.x + b.h, maxx);
			maxy = Math.Max(b.p.y + b.h, maxy);
		}
		
		min = new Pnt2D(minx, miny);
		max = new Pnt2D(maxx, maxy);
		
	}

        private int findBestAxis(List<Block> rigid) {
		
		Pnt2D center = new Pnt2D(max);
		center .Add(min);
		center .Times( 0.5);
		
		int xleft = 0;
		int xright = 0;
		int yleft = 0;
		int yright = 0;
		
		foreach (Block b in rigid) {
			if (b.p.x - b.h <= center.x) {
				xleft++;
			}
			if (b.p.x + b.h >= center.x) {
				xright++;
			}
			if (b.p.y - b.h <= center.y) {
				yleft++;
			}
			if (b.p.y + b.h >= center.x) {
				yright++;
			}
		}
		
		if (Math.Abs(xleft - xright) > Math.Abs(yleft - yright)) return 0;
		else return 1;
	}

        public AABBTree(List<Block> rigid, int d)
        {
            buildTree(rigid, d);
        }

        private void buildTree(List<Block> rigid, int d) {
		setBounds(rigid);
		blocks = rigid;
		depth = d;
		
		bool split = false;
		
		if (blocks.Count > minBlocks && depth < maxDepth) {
			split = true;
		}
		
		if (split) {
		
			int axis = findBestAxis(blocks);
			
			List<Block> leftBlocks = new List<Block>();
			List<Block> rightBlocks = new List<Block>();
			
			Pnt2D center = new Pnt2D(max);
			center .Add( min);
			center .Times (0.5);
			
			double sMid = (axis == 0 ? center.x : center.y);
			
			foreach (Block b in blocks) {
				
				double bLeft = (axis == 0 ? b.p.x : b.p.y) - b.h;
				double bRight = (axis == 0 ? b.p.x : b.p.y) + b.h;
				
				if (bLeft<=sMid) {
					leftBlocks.Add(b);
				}
				if (bRight >= sMid) {
					rightBlocks.Add(b);
				}
				
			}
			
			left = new AABBTree(leftBlocks, depth + 1);
			right = new AABBTree(rightBlocks, depth + 1);
			
		}
	}
    }
}
